#include "wheel.h"
#include "stdlib.h"
#include "math.h"
#include "sabertoothdrive.h"
#include "cmsis_os.h"


#define SaberTooth_1_ADDR 129
#define SaberTooth_2_ADDR 128
#define LEFTWHEEL 2
#define RIGHTWHEEL 1

/*
positive for forward;
negative for backward
*/
void  motor_wheel_driver(FOOD_CAR *car)
{
		
		
	commandToWheel(SaberTooth_1_ADDR,LEFTWHEEL,car->wheel_1_speed);
	HAL_Delay(1);
	commandToWheel(SaberTooth_1_ADDR,RIGHTWHEEL,car->wheel_2_speed);
	HAL_Delay(1);
	commandToWheel(SaberTooth_2_ADDR,LEFTWHEEL,car->wheel_3_speed);
	HAL_Delay(1);
	commandToWheel(SaberTooth_2_ADDR,RIGHTWHEEL,car->wheel_4_speed);
	
	HAL_GPIO_WritePin(Emegency_Stop_GPIO_Port,Emegency_Stop_Pin,GPIO_PIN_SET);
	

}
void motor_wheel_run(FOOD_CAR *car)
{
//	if(car->target_coord_atanNum>Min_angle)
//	{
//			car->wheel_1_speed=car->wheel_3_speed=car->run_speed;
//			car->wheel_2_speed=car->wheel_4_speed=car->angle_speed;	
//	}
//	else if(car->target_coord_atanNum<(Min_angle*(-1)))		
//	{
//		car->wheel_2_speed=car->wheel_4_speed=car->run_speed;
//		car->wheel_1_speed=car->wheel_3_speed=car->angle_speed;
//	}
//	else 
//		car->wheel_1_speed=car->wheel_2_speed=car->wheel_3_speed=car->wheel_4_speed=car->run_speed;
	
	motor_wheel_driver(car);

}

void motor_wheel_forward_setup (FOOD_CAR *car)
{
  car->wheel_1_speed=car->wheel_2_speed=car->wheel_3_speed=car->wheel_4_speed=car->run_speed;
	motor_wheel_run(car);

}
void motor_wheel_backward_setup (FOOD_CAR *car)
{

  car->wheel_1_speed=car->wheel_2_speed=car->wheel_3_speed=car->wheel_4_speed=car->run_speed*(-1);
  motor_wheel_run(car);
	
}
void motor_wheel_RightShift_setup (FOOD_CAR *car)
{
	car->wheel_1_speed=car->wheel_4_speed=car->run_speed;
	car->wheel_2_speed=car->wheel_3_speed=car->run_speed*(-1);
  motor_wheel_run(car);

}

void motor_wheel_LeftShift_setup (FOOD_CAR *car)
{
	car->wheel_1_speed=car->wheel_4_speed=car->run_speed*(-1);
	car->wheel_2_speed=car->wheel_3_speed=car->run_speed;
  motor_wheel_run(car);


}

void motor_slightly_turnLeft(FOOD_CAR *car)
{
	car->wheel_1_speed=car->wheel_4_speed=car->run_speed;
	car->wheel_2_speed=car->wheel_3_speed=car->angle_speed;
	motor_wheel_run(car);

}	
void motor_sharply_turnLeft(FOOD_CAR *car)
{

}
void motor_slightly_turnRight(FOOD_CAR *car)
{

}	
void motor_sharply_turnRight(FOOD_CAR *car)
{

}
void motor_wheel_CW_turn(FOOD_CAR *car)
{
//	if(car->target_coord_atanNum>0)
//	{
//			car->wheel_1_speed=car->wheel_3_speed=car->run_speed;
//			car->wheel_2_speed=car->wheel_4_speed=car->angle_speed;	
//	}
//	else if(car->target_coord_atanNum<0)
//		
//	{
//		car->wheel_2_speed=car->wheel_4_speed=car->run_speed;
//		car->wheel_1_speed=car->wheel_3_speed=car->angle_speed;
//	}
//	else
//		car->wheel_1_speed=car->wheel_2_speed=car->wheel_3_speed=car->wheel_4_speed=0;
		car->wheel_1_speed=car->wheel_3_speed=car->run_speed;
	  car->wheel_2_speed=car->wheel_4_speed=car->run_speed*(-1);
	
		motor_wheel_driver(car);	

}
void motor_wheel_CCW_turn(FOOD_CAR *car)
{
	car->wheel_1_speed=car->wheel_3_speed=car->run_speed*(-1);
	car->wheel_2_speed=car->wheel_4_speed=car->run_speed;
	motor_wheel_driver(car);
	
}


void motor_wheel_stop (FOOD_CAR *car)
{
	// rudeuce the speed smoothly
	motor_wheel_reduceSpeed(car);
	
	// Emergency stop
	
	HAL_GPIO_WritePin(Emegency_Stop_GPIO_Port,Emegency_Stop_Pin,GPIO_PIN_RESET);



}

void motor_wheel_reduceSpeed (FOOD_CAR *car)
{

	int difference,abs_difference[4];
	uint16_t min_abs_differ;
	int tap4,tap1,tap2,tap3;
	
	difference=car->wheel_1_speed;
	abs_difference[0]=abs(difference);
	tap1=difference/abs_difference[0];
	
	difference=car->wheel_2_speed;
	abs_difference[1]=abs(difference);
	tap2=difference/abs_difference[1];
	
	difference=car->wheel_3_speed;
	abs_difference[2]=abs(difference);
	tap3=difference/abs_difference[2];
	
	difference=car->wheel_4_speed;
	abs_difference[3]=abs(difference);
	tap4=difference/abs_difference[3];
	
	min_abs_differ=abs_difference[0];
	
	for(int a=1;a<4;a++)
	{
		if(min_abs_differ>abs_difference[a])
			min_abs_differ=abs_difference[a];	
	}
	
	car->wheel_1_speed=min_abs_differ*tap1;
	car->wheel_2_speed=min_abs_differ*tap2;
	car->wheel_3_speed=min_abs_differ*tap3;
	car->wheel_4_speed=min_abs_differ*tap4;
	
	
	
	for(uint16_t i=0;i<min_abs_differ;i++)
	{
		motor_wheel_driver(car);
		
		osDelay(1);
		car->wheel_1_speed-=tap1;
		car->wheel_2_speed-=tap2;
		car->wheel_3_speed-=tap3;
		car->wheel_4_speed-=tap4;

	}
  
	motor_wheel_driver(car);
	car->run_speed=0;
	car->angle_speed=0;
	

}




